 // define what pins the LEDs are connected to.

#define Red PB2
#define Yellow PB1
#define Green PB0

#define IdByte 0
#define TimerByte 1
#define LeftSpeed 2
#define RightSpeed 3
#define LeftDistH 4
#define LeftDistL 5
#define RightDistH 6
#define RightDistL 7
#define CommandByte 8

#define IrLeftH 1
#define IrLeftL 2
#define IrRightH 3
#define IrRightL 4
#define UltraH 5
#define UltraL 6
#define LeftAmpH 7
#define LeftAmpL 8
#define RightAmpH 9
#define RightAmpL 10
#define LeftEncoderH 11
#define LeftEncoderL 12
#define RightEncoderH 13
#define RightEncoderL 14
#define SensorWarning 15

//Macros for USARTs 9600 baud 8n1
 
#define FOSC 20000000
#define BAUD 9600
#define MYUBRR (((FOSC/16)/BAUD)-1)

// Some macros that make the code more readable
#define output_low(port,pin) port &= (uint8_t)~(1<<pin)
#define output_high(port,pin) port |= (uint8_t)(1<<pin)
#define set_input(portdir,pin) portdir &= (uint8_t)~(1<<pin)
#define set_output(portdir,pin) portdir |=(uint8_t)(1<<pin)

#define set(p,r) ((p) |= (bit(r)))
#define clear(p,r) ((p) &= ~(bit(r)))
#define bit(x) (0x01 << (x))

//Timer Macros

#define startTimer1 TCCR1B|=0x05 //1024 prescale
#define stopTimer1 TCCR1B&=~0x07

#define startTimer0 TCCR0B|=0x05 //1024 prescale
#define stopTimer0 TCCR0B&=~0x07

//Functions Prototypes
void Move(void);
void FakeData(void);
void SendSensorData(void);
void Debug(void);
void SendSensorData(void);
void ioinit(void);      // initializes IO
uint8_t uart_getdata(void);
uint8_t uart_getchar(void);
static int uart_putdata(char c);
static int uart_putchar(char c, FILE *stream);
static FILE mystdout = FDEV_SETUP_STREAM(uart_putchar, NULL, _FDEV_SETUP_WRITE);

//Global Var's

volatile uint8_t TimerCount;
volatile uint16_t LOS;
volatile uint8_t Override;
volatile uint8_t DataPacket[9];
volatile uint8_t sensorArray[16];
volatile int8_t CalByte=23;
volatile uint8_t CalibrateNow;

// Functions
// this is just a program that 'kills time' in a calibrated method
 void delay_ms(uint8_t ms) {
   uint16_t delay_count = F_CPU / 17500;
   volatile uint16_t i;
 
   while (ms != 0) {
     for (i=0; i != delay_count; i++);
     ms--;
   }
}


void ioinit (void){
    set_output(DDRB, Yellow );
   	set_output(DDRB, Red);
   	set_output(DDRB, Green);

    //USART 1 Baud rate: 9600
    UBRR1H = (MYUBRR>>8);
    UBRR1L = MYUBRR;
    UCSR1B = (1<<RXEN1)|(1<<TXEN1);

    //USART 0 Baud rate: 9600
    UBRR0H = (MYUBRR>>8);
    UBRR0L = MYUBRR;
    UCSR0B = (1<<RXEN0)|(1<<TXEN0);

    
    stdout = &mystdout; //Required for printf init
}


static int uart_putchar(char c, FILE *stream){

    if (c == '\n'){
	loop_until_bit_is_set(UCSR1A, UDRE1); 
    UDR1 = '\r';
	} 
    loop_until_bit_is_set(UCSR1A, UDRE1);
    UDR1 = c;
    
    return 0;
}


uint8_t uart_getchar(void){
    while(!(UCSR1A & (1<<RXC1)) );
    return(UDR1);
}


static int uart_putdata(char c){ 
    loop_until_bit_is_set(UCSR0A, UDRE0);
    UDR0 = c;
   
	return 0;
}


uint8_t uart_getdata(void){
    while(!(UCSR0A & (1<<RXC0)) );
    return(UDR0);
}


void Debug(void){
    if(DataPacket[IdByte]=='V'){
        
        if ((DataPacket[2] >= 'A' && DataPacket[2] <= 'Z') && (DataPacket[3] >= 'A' && DataPacket[3] <= 'Z')){
            //forward
            printf("Forward\n");
        } else if ((DataPacket[2] >= 'a' && DataPacket[2] <= 'z') && (DataPacket[3] >= 'a' && DataPacket[3] <= 'z')){
            //revrse
            printf("Reverse\n");
        } else if ((DataPacket[2] >= 'a' && DataPacket[2] <= 'z') && (DataPacket[3] >= 'A' && DataPacket[3] <= 'Z')){
            //Left Turn
            printf("LeftTrn\n");
        } else if ((DataPacket[2] >= 'A' && DataPacket[2] <= 'Z') && (DataPacket[3] >= 'a' && DataPacket[3] <= 'z')){
            //Right Turn
            printf("RightTn\n");
        } else if ((DataPacket[2] == '!' && DataPacket[3] == '!')){
            printf("Break\n");
        } else if ((DataPacket[2] == '$' && DataPacket[3] == '$')){
            printf("Sync\n");
        } else {
            printf("Stop!\n");
        }
    }
}


void SendSensorData(void){
    for(uint8_t i = 0;i<15;i++){
        uart_putdata(sensorArray[i]);
	}
}

void Calibrate(void){
int leftEncoder = sensorArray[LeftEncoderH] << 8;
leftEncoder += sensorArray[LeftEncoderL];

int rightEncoder = sensorArray[RightEncoderH] << 8;
rightEncoder += sensorArray[RightEncoderL];

int Diff = leftEncoder-rightEncoder;
if(Diff >10){
CalByte+=5;
}else if(Diff <-10){
CalByte-=5;
}else if(Diff >5){
CalByte+=1;
}else if(Diff <-5){
CalByte-=1;
}


}


